#include "raycastvehicle.h"
// #include "src/utils/bullet/BulletDynamics/Vehicle/btWheelInfo.h"

bRaycastVehicle::bRaycastVehicle( const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster )
 : btRaycastVehicle( tuning, chassis, raycaster ), forwardWheelSumSide(0), touchingfrontwheels(0)
{
}

btScalar bRaycastVehicle::calcRollingFriction(btWheelContactPoint& contactPoint)
{
	const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld;

	btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition(); 
	btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition();
	btScalar maxImpulse  = contactPoint.m_maxImpulse;
	
	btVector3 vel = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1) - contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2);
	btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel);

	// calculate j that moves us to zero relative velocity
	btScalar j1 = -vrel * contactPoint.m_jacDiagABInv;
	btSetMin(j1, maxImpulse);
	btSetMax(j1, -maxImpulse);

	return j1;
}

void bRaycastVehicle::updateVehicle( btScalar step )
{
	for ( int i = 0; i < getNumWheels(); i++ )
		updateWheelTransform(i,false);

	int numWheel = getNumWheels();
	if ( numWheel==0 )
		return;

	const btTransform& chassisTrans = getChassisWorldTransform();

	m_forwardWS.resize(numWheel);
	m_axle.resize(numWheel);
	m_forwardImpulse.resize(numWheel);
	m_sideImpulse.resize(numWheel);

	// reset forward & side impulses
	for ( int i=0; i < numWheel; i++ )
	{
		m_sideImpulse[i] = btScalar(0.);
		m_forwardImpulse[i] = btScalar(0.);
	}
	
	// CALCULATE km/h
		m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length();
		btVector3 forwardW (
			chassisTrans.getBasis()[0][m_indexForwardAxis],
			chassisTrans.getBasis()[1][m_indexForwardAxis],
			chassisTrans.getBasis()[2][m_indexForwardAxis]
		);

		if ( forwardW.dot(getRigidBody()->getLinearVelocity()) > btScalar(0.) )
			m_currentVehicleSpeedKmHour *= btScalar(-1.);

	// SIMULATE SUSPENSION
		for ( int i = 0; i < numWheel; i++ )
			customrayCast( m_wheelInfo[i], i); // FIXME remove averages after interpolation works

		updateSuspension(step);


	// APPLY SUSPENSION FORCES TO CHASSIS
		for ( int i = 0; i < numWheel; i++ )
		{
			//apply suspension force
			btWheelInfo& wheel = m_wheelInfo[i];

			btScalar suspensionForce = wheel.m_wheelsSuspensionForce;

			if (suspensionForce > wheel.m_maxSuspensionForce)
				suspensionForce = wheel.m_maxSuspensionForce;

			btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
			btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();

			getRigidBody()->applyImpulse(impulse, relpos);
		}


	for ( int i=0; i < numWheel; i++ )
	{
		btWheelInfo& wheelInfo = m_wheelInfo[i];

		btRigidBody* groundObject = static_cast<btRigidBody*>(wheelInfo.m_raycastInfo.m_groundObject);

		if (groundObject)
		{
			const btTransform& wheelTrans = getWheelTransformWS( i );

			btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
			m_axle[i] = btVector3( wheelBasis0[0][m_indexRightAxis], wheelBasis0[1][m_indexRightAxis], wheelBasis0[2][m_indexRightAxis] );
			
			const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
			btScalar proj = m_axle[i].dot(surfNormalWS);
			m_axle[i] -= surfNormalWS * proj;
			m_axle[i] = m_axle[i].normalize();
			
			m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
			m_forwardWS[i].normalize();
		
			// resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects 
			resolveSingleBilateral(
				*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,	// body1, position1
				*groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,	// body2, position2
				btScalar(0.), m_axle[i],m_sideImpulse[i],step			// distance, normal, impulse, timestep
			);

// 			m_sideImpulse[i] *= btScalar(1.);
		}
	}

	forwardWheelSumSide = 0.;
	touchingfrontwheels = 0.;

	for ( int wheel=0; wheel < numWheel; wheel++ )
	{
		btWheelInfo& wheelInfo = m_wheelInfo[wheel];
		btRigidBody* groundObject = static_cast<btRigidBody*>(wheelInfo.m_raycastInfo.m_groundObject);

		// THROTTLE AND BRAKING
		btScalar rollingFriction = 0.f;

		if (wheelInfo.m_engineForce != 0.f)
		{
			if (groundObject)
			{
				rollingFriction -= wheelInfo.m_engineForce * step;
			}

// 			btVector3 relpos = wheelInfo.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
// 			btVector3 vel = btVector3(wheelInfo.m_engineForce/7000,0,0);
// // 			cerr << "2 -" << wheel << " vel: x " << vel.getX() << " y " << vel.getY() << " z " << vel.getZ() << " sum " << vel.getX()+vel.getY()+vel.getZ() << endl;
// 			btVector3 fwd (
// 				chassisTrans.getBasis()[0][m_indexForwardAxis],
// 				chassisTrans.getBasis()[1][m_indexForwardAxis],
// 				chassisTrans.getBasis()[2][m_indexForwardAxis]
// 			);
// 
// 			btScalar proj = fwd.dot(wheelInfo.m_raycastInfo.m_contactNormalWS);
// 			fwd -= wheelInfo.m_raycastInfo.m_contactNormalWS * proj;
// 
// 			btScalar proj2 = fwd.dot(vel);
// 
// 			wheelInfo.m_deltaRotation += (proj2 * step) / (wheelInfo.m_wheelsRadius);
		}

// 		wheelInfo.m_rotation += wheelInfo.m_deltaRotation;
// 		wheelInfo.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact
// 		cout << wheelInfo.m_deltaRotation << endl;
// 		cerr << "back wheel.m_rotation: " << wheelInfo.m_rotation << endl;

		if (wheelInfo.m_brake != 0.f)
		{
			if (groundObject)
			{
				btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel], wheelInfo.m_brake*step );
				rollingFriction += calcRollingFriction(contactPt);
			}
		}

		// sliding & force feedback info
		m_forwardImpulse[wheel] = btScalar(0.f);
// 		float previousSkidInfo = m_wheelInfo[wheel].m_skidInfo;
		m_wheelInfo[wheel].m_skidInfo = btScalar(1.0f);

		if (groundObject)
		{
// 			cerr << groundObject->getFriction() << endl;
			m_sideImpulse[wheel] *= groundObject->getFriction();
			m_forwardImpulse[wheel] = rollingFriction;

			btScalar maximp = wheelInfo.m_wheelsSuspensionForce * step * wheelInfo.m_frictionSlip;// * groundObject->getFriction(); //  
			btScalar maximpSquared = maximp * maximp;

			btScalar x = m_forwardImpulse[wheel];
			btScalar y = m_sideImpulse[wheel];

			btScalar impulseSquared = (x*x + y*y);

			// sliding?
			if (impulseSquared > maximpSquared)
			{
				btScalar factor = maximp / btSqrt(impulseSquared);

				m_wheelInfo[wheel].m_skidInfo *= factor;
				
				// apply skidinfo to impulses
// 				if (m_sideImpulse[wheel] != btScalar(0.f))
					if (m_wheelInfo[wheel].m_skidInfo < btScalar(1.f))
					{
						m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
						m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
					}
			}

			if ( getSteeringValue(wheel) != 0 ) // FIXME -> hassteeringinfluence
			{
				forwardWheelSumSide += m_sideImpulse[wheel]; //  * m_wheelInfo[wheel].m_skidInfo for expoonential
// cerr << forwardWheelSumSide << endl;
// 				touchingfrontwheels += 1;
			}
		} 
// cerr << m_wheelInfo[wheel].m_skidInfo << endl;
	}


	// apply the impulses
	for ( int wheel = 0; wheel < numWheel; wheel++ )
	{
		btWheelInfo& wheelInfo = m_wheelInfo[wheel];

		btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - m_chassisBody->getCenterOfMassPosition();

		if (m_forwardImpulse[wheel] != btScalar(0.f))
			m_chassisBody->applyImpulse( m_forwardWS[wheel] * m_forwardImpulse[wheel], rel_pos);

		if ( m_sideImpulse[wheel] != btScalar(0.f) )
		{
			btRigidBody* groundObject = static_cast<btRigidBody*>(wheelInfo.m_raycastInfo.m_groundObject);
			btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - groundObject->getCenterOfMassPosition();
			btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];

			rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
			m_chassisBody->applyImpulse(sideImp,rel_pos);

			//apply friction impulse on the ground
			groundObject->applyImpulse(-sideImp,rel_pos2);
		}
	}

	if ( touchingfrontwheels > 0 )
		forwardWheelSumSide /= touchingfrontwheels;

// 		cerr << "side: " << forwardWheelSumSide << endl;
// 		cerr << "forw: " << forwardWheelSumForw << endl << endl;

	// calculate wheel rotation
	for ( int i = 0; i < numWheel; i++ )
	{
		btWheelInfo& wheel = m_wheelInfo[i];

		if ( wheel.m_raycastInfo.m_isInContact )
		{
			btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
			btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos );
// 			cerr << "vel: x " << vel.getX() << " y " << vel.getY() << " z " << vel.getZ() << " sum " << vel.getX()+vel.getY()+vel.getZ();

			btVector3 fwd (
				chassisTrans.getBasis()[0][m_indexForwardAxis],
				chassisTrans.getBasis()[1][m_indexForwardAxis],
				chassisTrans.getBasis()[2][m_indexForwardAxis]
			);

			btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
			fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;

			btScalar proj2 = fwd.dot(vel);

			wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);

		}

		wheel.m_rotation += wheel.m_deltaRotation;
		wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact

// 		cerr << "front  wheel.m_rotation: " << wheel.m_rotation << endl;
	}
}

btScalar bRaycastVehicle::customrayCast(btWheelInfo& wheel, unsigned int cwheel)
{
	updateWheelTransformsWS( wheel,false);

	btScalar depth = -1;
	
	btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius;

	btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
	const btVector3& source = wheel.m_raycastInfo.m_hardPointWS;
	wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
	const btVector3& target = wheel.m_raycastInfo.m_contactPointWS;

	btScalar param = btScalar(0.);
	
	btVehicleRaycaster::btVehicleRaycasterResult	rayResults;

	btAssert(m_vehicleRaycaster);

	void* object = m_vehicleRaycaster->castRay(source,target,rayResults);

	wheel.m_raycastInfo.m_groundObject = 0;

	if (object)
	{
		param = rayResults.m_distFraction;
		depth = raylen * rayResults.m_distFraction;
		
// 		Bodypart* bp = static_cast<Bodypart*>(object);

		// INTERPOLATE
			// bullet func, doesn't work?
	// 		void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
			if ( interpolateInfo[cwheel].currnormal == rayResults.m_hitNormalInWorld )
			{
				if ( interpolateInfo[cwheel].currpolycount < 50 )
					interpolateInfo[cwheel].currpolycount++;
			}
			else
			{
	// 			interpolateInfo[cwheel].prevpolycount = (interpolateInfo[cwheel].currpolycount * 2)+1;
				interpolateInfo[cwheel].prevpolycount = interpolateInfo[cwheel].currpolycount;
				interpolateInfo[cwheel].currpolycount = 1;
				interpolateInfo[cwheel].prevnormal = interpolateInfo[cwheel].currnormal;
				interpolateInfo[cwheel].currnormal = rayResults.m_hitNormalInWorld;
			}

			float factor;
			if ( interpolateInfo[cwheel].prevpolycount <= interpolateInfo[cwheel].currpolycount )
				factor = 1;
			else
				factor = (1.0f/interpolateInfo[cwheel].prevpolycount) * interpolateInfo[cwheel].currpolycount;
			
// 			cerr << factor << endl;

			wheel.m_raycastInfo.m_contactNormalWS = (factor*interpolateInfo[cwheel].currnormal) + ((1.0f-factor)*interpolateInfo[cwheel].prevnormal);

// 		cerr << "x: " << wheel.m_raycastInfo.m_contactNormalWS.getX() << endl;
// 		if (cwheel == 0)
// 			cerr << "y: " << wheel.m_raycastInfo.m_contactNormalWS.getY() << endl;
// 		cerr << "z: " << wheel.m_raycastInfo.m_contactNormalWS.getZ() << endl << endl;

		wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld;


// 		smoothing with averages
// 		if ( normalAvg.size() > 2 )
// 			normalAvg.erase(normalAvg.beg/*/*());
// 
// 		normalAvg.push_back( rayResults.m_hitNormalInWorld );
// 		btVector3 sum(0,0,0);
// 		for ( unsigned int i=0; i < normalAvg.size(); i++ )
// 			sum += normalAvg[i];
// 
// 		sum /= normalAvg.size();
// 		wheel.m_raycastInfo.m_contactNormalWS = sum;
// 		normalAvg.push_back( rayResults.m_hitNormalInWorld );
// 		cerr << "x: " << sum.getX() << endl;
// 		cerr << "y: " << sum.getY() << endl;
// 		cerr << "z: " << sum.getZ() << endl << endl;

// 		while ( normalAvg.size() > 1 )
// 			normalAvg.erase(normalAvg.begin());
// 		if ( normalAvg.size() == 1 )
// 		{
// 			float dim = 0.8f;
// 			wheel.m_raycastInfo.m_contactNormalWS = ((normalAvg[0]*dim) + (rayResults.m_hitNormalInWorld*(1.0f-dim)));
// 			normalAvg.push_back( wheel.m_raycastInfo.m_contactNormalWS );
// 		}
// 		else
// 		{
// 			wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld;
// 			normalAvg.push_back( wheel.m_raycastInfo.m_contactNormalWS );
// 		}

		
		wheel.m_raycastInfo.m_isInContact = true;
		
		//wheel.m_raycastInfo.m_groundObject = &getFixedBody();///@todo for driving on dynamic/movable objects!;
		wheel.m_raycastInfo.m_groundObject = object;


		btScalar hitDistance = param*raylen;
		wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius;
		
		//clamp on max suspension travel
		btScalar  minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01);
		btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01);

		if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
			wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;

		if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength)
			wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;

		wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;

		btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS );

		btVector3 chassis_velocity_at_contactPoint;
		btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition();

		chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos);

		btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );

		if ( denominator >= btScalar(-0.1))
		{
			wheel.m_suspensionRelativeVelocity = btScalar(0.0);
			wheel.m_clippedInvContactDotSuspension = 10;
		}
		else
		{
			btScalar inv = btScalar(-1.) / denominator;
			wheel.m_suspensionRelativeVelocity = projVel * inv;
			wheel.m_clippedInvContactDotSuspension = inv;
		}
			
	}
	else
	{
		//put wheel info as in rest position
		wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
		wheel.m_suspensionRelativeVelocity = btScalar(0.0);
		wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
// 		wheel.m_clippedInvContactDotSuspension = 10;


		interpolateInfo[cwheel].currpolycount = 1;
		interpolateInfo[cwheel].prevpolycount = 1;
		interpolateInfo[cwheel].currnormal = btVector3(0,0,0);
		interpolateInfo[cwheel].prevnormal = btVector3(0,0,0);;
		wheel.m_raycastInfo.m_contactNormalWS = interpolateInfo[cwheel].currnormal;

// 		if ( normalAvg.size() > 2 )
// 			normalAvg.erase(normalAvg.begin());

// 		while ( normalAvg.size() > 1 )
// 			normalAvg.erase(normalAvg.begin());
// 		if ( normalAvg.size() == 1 )
// 		{
// 			float dim = 0.8f;
// 			wheel.m_raycastInfo.m_contactNormalWS = ((normalAvg[0]*dim) + (-wheel.m_raycastInfo.m_wheelDirectionWS*(1.0f-dim)));
// 			normalAvg.push_back( -wheel.m_raycastInfo.m_wheelDirectionWS );
// 		}
// 		else
// 		{
// 			wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
// 			normalAvg.push_back( -wheel.m_raycastInfo.m_wheelDirectionWS );
// 		}
	}

	return depth;
}



